/** @file working_timer.c
 *
 * @author cameris <camerisdump@gmail.com>
 */

#include "working_timer.h"
#include <stdint.h>
#include <avr/interrupt.h>

static void (*wtbuf[MAX_WORKERS])(void);
static uint8_t wtcnt = 0;

#define WTIMER_PRESCALE 8
#define WTIMER_CALC_OCR (uint16_t) (((F_CPU * WTIMER_MS)/WTIMER_PRESCALE)-1)

/** @brief initializes the timer */
void wtimer_init(void)
{
	WTIMER_TIMERB |= (1<<WTIMER_WGM2)|(1<<WTIMER_CS1);
	WTIMER_INT_REG |= (1<<WTIMER_INT);
	WTIMER_OCRH = (WTIMER_CALC_OCR>>8);
	WTIMER_OCRL = (WTIMER_CALC_OCR & 0xFF);
}

/** @brief add a function to the timer
 * @param worker the function to add */
void wtimer_add(void (*worker)(void))
{
	uint_fast8_t add = 1;
	/* check if function is already in buffer */
	for (uint_fast8_t i=0; i<wtcnt; i++) {
		if (wtbuf[i] == worker) {
			add = 0;
		}
	}
	if (add) {
		wtbuf[wtcnt] = worker;
		wtcnt++;
	}
}

/** @brief timer interrupt routine
 *
 * working timer interrupt routine, calls the added functions */
ISR(WTIMER_INT_VECT)
{
	/* call all functions */
	for (uint_fast8_t i=0; i<wtcnt; i++) {
		(wtbuf[i])();
	}
}
